
import time
import _thread
import ustruct as struct
import math
from machine import I2C, UART, Pin, PWM, Timer, ADC

from DShotPIO import DShotPIO, DSHOT_SPEEDS
from mpudmp import MPU6050
from funcs import remap, readCmd, initDMP

from pid import PID

dpsCounter = 0

'''
def feedThread(self):
    global dpsCounter
    print("dps: %d"%dpsCounter)
    dpsCounter = 0

timer = Timer()         # global, single timer
timer.init(period=1000, mode=Timer.PERIODIC, callback=feedThread)
'''

pitch = 0
roll = 0
yaw = 0

gyro_x = 0
gyro_y = 0
gyro_z = 0

def run_on_core1():
    
    from machine import I2C, Pin
    import mpudmp
    
    fifoFull = Pin(16, Pin.IN)

    i2c = I2C(id=1, sda=Pin(14), scl=Pin(15), freq=1000_000)

    device_address = 0x68
    freq_divider = 0x04  # freq = 200Hz / (1 + value)  推荐 >=4

    # Make an MPU6050
    mpu = mpudmp.MPU6050(i2c, device_address, freq_divider)

    # Initiate your DMP
    mpu.dmp_initialize()
    mpu.set_DMP_enabled(True)

    packet_size = mpu.DMP_get_FIFO_packet_size()
    FIFO_buffer = [0]*64

    global pitch, roll, yaw, gyro_x,gyro_y,gyro_z, dpsCounter

    while True:
        
        if (fifoFull.value()):
            FIFO_buffer = mpu.get_FIFO_bytes_block(packet_size) # get all the DMP data here
            
            gyro_x,gyro_y,gyro_z = mpu.DMP_get_gyro_int16(FIFO_buffer)
        
            qw,qx,qy,qz = mpu.DMP_get_quaternion_int16(FIFO_buffer)
            #grav_x,grav_y,grav_z = mpu.DMP_get_gravity(qw,qx,qy,qz)
            roll, pitch, yaw = mpu.DMP_get_euler_roll_pitch_yaw(qw,qx,qy,qz)

            #print("roll: {}, pitch: {}, yaw: {}".format(roll, pitch, yaw))

def dutyScale(middle, val):
    return int((val - middle) / 2 + middle)


def run_on_core0():
    
    # Create the DShot object 
    dshot = DShotPIO(stateMachineID=0, outputPin=17, dshotSpeed=DSHOT_SPEEDS.DSHOT600)

    # Most ESCs need a period of zero throttle to arm

    throttle = 48

    '''
    for _ in range(100):
        dshot.sendThrottleCommand(48)
        time.sleep_ms(10)
    '''
    
    pins = [
        {'pin': 23, 'freq': 100},#原来的17 pin不动
        {'pin': 18, 'freq': 100},
        {'pin': 19, 'freq': 100},
        {'pin': 20, 'freq': 100},
        {'pin': 22, 'freq': 100}
    ]
    for pitem in pins:
        pwm = PWM(Pin(pitem['pin'], Pin.OUT))
        pwm.freq(pitem['freq'])
        pitem['pwm'] = pwm

    adcVol = ADC(Pin(29))

    # PWM占空比控制
    freq = 100
    cycle = 1000 / freq

    dutyAll = 65535
    dutyStart = int(dutyAll * (0.5 / cycle))
    dutyEnd = int(dutyAll * (2.5 / cycle))
    dutyMiddle = int((dutyStart + dutyEnd) / 2)
    dutyRange = dutyEnd - dutyStart
    dutyHalf = dutyRange / 2

    failTimes = 0
    
    #自稳模式标志，0自稳，1增稳
    modeExtendStable = 0
    
    #自稳模式角度限制
    stableLimitAngle = 45
    #角速度模式限制角速度（rad/s 角度每秒）
    stableLimitGyro = 720
    
    #活动范围打折
    limit = dutyRange / 2 * 0.5
    
    pidRoll = PID(kp=1.05, ki=0, kd=0.3, limit=limit, offset=dutyMiddle)	# d:消除零界点的抖动
    pidPitch = PID(kp=1.05, ki=0, kd=0.3, limit=limit, offset=dutyMiddle)

    global pitch, roll, yaw, gyro_x,gyro_y,gyro_z, dpsCounter
    
    v0 = 0
    v2 = 0
    v3 = 0
    
    #安装误差偏置
    offsetRoll = -4
    offsetPitch = 0
    
    #升力偏置数
    liftCoefficient = 0
    
    uart = UART(1, baudrate = 115200,bits = 8,parity = None,stop = 1,
         timeout=10, timeout_char=1, tx = Pin(8),rx = Pin(9))

    while True:
                
        ''' ###################################
        电池电压保护 8.4V 停止工作， = 2.8 * 3
            ###################################'''
        batteryU16 = adcVol.read_u16()
        batteryVol = batteryU16 / 65535 * 3.3 * 6

        vals = readCmd(uart)
        
        if vals:
            
            (v0,v1,v2,v3, cmd) = vals

            # Get the potentiometer value and remap it to 48 - 2047
            #throttleTarget = int(remap(v0, 0, 65536, 48, 2047))
            #print(throttleTarget)
            
            v1 = dutyEnd - v1 + dutyStart
            pins[3]['pwm'].duty_u16(dutyScale(dutyMiddle, v1))
            
            #返回电压
            if(cmd & 0b01):
                uart.write("{:04x}\n".format(batteryU16))
            
            #自稳模式切换
            if(cmd & 0b10):
                modeExtendStable = 1
            else:
                modeExtendStable = 0
            
            #升力切换
            if(cmd & 0b100):
                liftCoefficient = 1500
            else:
                liftCoefficient = 0
                
            #print("v0-v3:{} {} {} {}".format(v0,v1,v2,v3))

            failTimes = 0
        else:
            failTimes +=1
        
        # Get the potentiometer value and remap it to 48 - 2047
        throttleTarget = int(remap(v0, 0, 65536, 48, 2047))
        #print(throttleTarget)
        
        # 油门渐变，突变容易伤电机和电调
        if(throttleTarget > throttle):
            throttle += 1
        elif(throttleTarget < throttle):
            throttle -= 1
            
        #电压低，保护电池，不允许启动电机
        if(batteryVol < 8.4):
            v0 = 0
            #pins[0]['pwm'].duty_u16(v0)
    
        #多次没收到信号，关闭油门, 使用自稳
        if failTimes > 5000:
            #pins[0]['pwm'].duty_u16(0)
            v0 = 0
            #print("no signal")
            v2 = dutyMiddle
            v3 = dutyMiddle

        if v0 > 4500:
            dshot.sendThrottleCommand(throttle)
        elif v0 > 3100:
            dshot.sendThrottleCommand(0)	#连续发送0 是解锁
            
        targetRoll = -(v2 - dutyMiddle)
        targetPitch = (v3 - dutyMiddle)

        #增稳模式，超出临界点转为角速度控制
        if modeExtendStable == 1:
            
            #if abs(targetRoll) > dutyHalf * 0.2:
            #x轴 角速度控制 非线性，二次函数曲线
            tmpRoll = math.sqrt(abs(targetRoll) / dutyHalf) * targetRoll
            currentXspeed = -gyro_x * dutyHalf / stableLimitGyro
            dutyRoll = pidRoll.update(currentXspeed, tmpRoll)

            #if abs(targetPitch) > dutyHalf * 0.2:
            #y轴 角速度控制 非线性，二次函数曲线
            tmpPitch = math.sqrt(abs(targetPitch) / dutyHalf) * targetPitch
            currentYspeed = -gyro_y * dutyHalf / stableLimitGyro
            dutyPitch = pidPitch.update(currentYspeed, tmpPitch)
        else:
                    
            #PID 应以中点值作为调整基准， 数学期望为0
            '''
            第二个参数值为 13107/2 的范围，将角速度限制在45°的话，45应该乘以系数到达13107/2=6553.5
            则roll 应乘以系数：6553.5 / 45
            '''
            currentRoll = -(roll + offsetRoll) * dutyHalf / stableLimitAngle
            dutyRoll = pidRoll.update(currentRoll, targetRoll)
            
            #y 轴重力姿态
            currentPitch = -(pitch + offsetPitch) * dutyHalf / stableLimitAngle
            dutyPitch = pidPitch.update(currentPitch, targetPitch)
            
        
        #横滚差分，需要进一步缩减运动范围
        dutyRoll = int(max(-limit * 0.7, min(dutyRoll, limit * 0.7)))
        dutyPitch = int(max(-limit, min(dutyPitch, limit)))
 
        #pins[1]['pwm'].duty_u16(dutyMiddle)
        #pins[2]['pwm'].duty_u16(dutyMiddle)
        pins[1]['pwm'].duty_u16(dutyRoll + dutyMiddle - liftCoefficient)
        pins[2]['pwm'].duty_u16(dutyRoll + dutyMiddle + liftCoefficient)
        pins[4]['pwm'].duty_u16(dutyPitch + dutyMiddle)
        
        dpsCounter+=1


#睡眠等待三秒以方便开发调试
time.sleep(3)
_dmpt = _thread.start_new_thread(run_on_core1, ())
run_on_core0()
